
#include "mod/mod_canopen.h"
#include "bsp/bsp_can1.h"

typedef struct
{
     uint16_t   id;
     uint16_t   fun_code;
     uint16_t   ts;
     uint8_t    len;

     uint8_t    cmd;

     uint16_t   index;

     uint8_t    subindex;

     uint32_t   data;
}CanOpenMsg;

static void MsgtoFrame(CanOpenMsg* msg, struct rt_can_msg *frame)
{
    frame->ide = 0;                  //标准帧
    frame->rtr = 0;                 //数据帧
    frame->len = msg->len;          //数据长度
    frame->id = msg->id | (msg->fun_code);
    frame->data[0] = msg->cmd;
    frame->data[1] = msg->index & 0xff;
    frame->data[2] = msg->index >> 8;
    frame->data[3] = msg->subindex;

    frame->data[4] = msg->data & 0xff;
    frame->data[5] = (msg->data >> 8) & 0xff;
    frame->data[6] = (msg->data >> 16) & 0xff;
    frame->data[7] = (msg->data >> 24) & 0xff;

}

static void FramtoMsg(struct rt_can_msg* frame, CanOpenMsg* msg)
{
    if (frame->ide == 0 && frame->rtr == 0)
    {

        msg->fun_code = frame->id & 0xff80;
        msg->id = frame->id & 0x7f;

        msg->cmd = frame->data[0];
        msg->index = (rt_uint16_t) ((frame->data[2] << 8) + frame->data[1]);
        msg->subindex = frame->data[3];
        msg->data = (rt_uint32_t) ((frame->data[7] << 24) + (frame->data[6] << 16) + (frame->data[5] << 8)
                + frame->data[4]);
    }
}

bool mod_canopen_write_sdo(uint16_t m_id, uint32_t address, uint8_t type, uint32_t data)
{
    int retry = 1;
    struct rt_can_msg tx_frame, rx_frame;
    bool status;

    uint16_t index = (uint16_t) (address >> 8);
    uint8_t subindex = (uint8_t) (address & 0x7f);

    tx_frame.ide = RT_CAN_STDID;        // 标准格式
    tx_frame.rtr = RT_CAN_DTR;          // 数据帧
    tx_frame.len = 8;                   // 数据长度
    tx_frame.id = m_id | RXSDO;

    memset(tx_frame.data, 0, sizeof(tx_frame.data));

    tx_frame.data[0] = type;
    tx_frame.data[1] = index & 0xff;
    tx_frame.data[2] = index >> 8;
    tx_frame.data[3] = subindex;

    tx_frame.data[4] = data & 0xff;
    tx_frame.data[5] = (data >> 8) & 0xff;
    tx_frame.data[6] = (data >> 16) & 0xff;
    tx_frame.data[7] = (data >> 24) & 0xff;


    while(retry)
    {
        bsp_can2_tx_transmit(&tx_frame);

        rt_tick_t timeout = 100;
        rt_tick_t ts = rt_tick_get_millisecond();
        rt_tick_t interval = 0;

        do
        {
            bsp_can2_rx_transmit(&rx_frame, timeout - interval);

            interval = rt_tick_get_millisecond() - ts;

            if(rx_frame.id == m_id || TXSDO && rx_frame.data[0] == WRITE_REP)
            {
                rt_thread_mdelay(10);
                return true;
            }
        } while ( interval < timeout );

        rt_thread_mdelay(10);

        retry--;
    }

    return false;
}

bool mod_canopen_read_sdo(uint16_t m_id, uint32_t address, uint8_t type, uint32_t *data)
{
    if (data == NULL)
        return false;

    int retry = 1;
    struct rt_can_msg tx_frame, rx_frame;
    bool status;

    uint16_t index = (uint16_t) (address >> 8);
    uint8_t subindex = (uint8_t) (address & 0x7f);

    tx_frame.ide = RT_CAN_STDID;        // 标准格式
    tx_frame.rtr = RT_CAN_DTR;          // 数据帧
    tx_frame.len = 8;                   // 数据长度
    tx_frame.id = m_id | RXSDO;

    memset(tx_frame.data, 0, sizeof(tx_frame.data));

    tx_frame.data[0] = READ_REQ;
    tx_frame.data[1] = index & 0xff;
    tx_frame.data[2] = index >> 8;
    tx_frame.data[3] = subindex;

    while(retry)
    {
        bsp_can2_tx_transmit(&tx_frame);

        rt_tick_t timeout = 100;
        rt_tick_t ts = rt_tick_get_millisecond();
        int interval = 0;

        do
        {
            bsp_can2_rx_transmit(&rx_frame, timeout - interval);

            interval = rt_tick_get_millisecond() - ts;
            if(interval < 0)
                interval = 0;

            if(rx_frame.id == m_id || TXSDO && rx_frame.data[0] == type)
            {
                uint32_t t = (uint32_t) ((rx_frame.data[7] << 24) + (rx_frame.data[6] << 16) +
                                         (rx_frame.data[5] << 8) + rx_frame.data[4]);
                *data = t;
                return true;
            }
        } while ( interval < timeout );

        retry--;
    }

    return false;
}

bool mod_canopen_start_node(int8_t id)
{
    struct rt_can_msg frame = { 0 };
    frame.len = 2;
    frame.ide = RT_CAN_STDID;        // 标准格式
    frame.rtr = RT_CAN_DTR;          // 数据帧
    frame.rsv = 0;
    frame.id = 0;
    frame.data[0] = 1;
    frame.data[1] = id;
    return bsp_can2_tx_transmit(&frame);
}

bool mod_canopen_sync(void)
{
    struct rt_can_msg frame = { 0 };
    frame.len = 0;
    frame.ide = RT_CAN_STDID;        // 标准格式
    frame.rtr = RT_CAN_DTR;          // 数据帧
    frame.rsv = 0;
    frame.id = 0x80;
    return bsp_can2_tx_transmit(&frame);
}

bool mod_canopen_write_rxpdo(uint16_t m_id, enum CID_FUNC channel, uint8_t *data, uint8_t dsize)
{
    struct rt_can_msg frame = { 0 };
    frame.len = dsize;
    frame.rtr = 0;
    frame.ide = 0;
    frame.rsv = 0;
    frame.id = ((uint32_t) channel) + m_id;
    memcpy(frame.data, data, dsize);
    return bsp_can2_tx_transmit(&frame);
}

